//
// File: ChordalL1Mean.cpp
//
// MATLAB Coder version            : 5.1
// C/C++ source code generated on  : 21-Feb-2021 21:16:52
//

// Include Files
#include "ChordalL1Mean.h"
#include "ChordalL1Mean_data.h"
#include "ChordalL1Mean_initialize.h"
#include "ChordalL1Mean_types.h"
#include "rand.h"
#include "rt_nonfinite.h"
#include "sort.h"
#include "svd.h"
#include "rt_nonfinite.h"
#include <cmath>
#include <cstring>

// Function Definitions
//
// 1. Initialize
// Arguments    : const cell_wrap_0 R_input[2]
//                boolean_T b_outlier_rejection
//                double n_iterations
//                double thr_convergence
//                double R[9]
// Return Type  : void
//
cell_wrap_0 ChordalL1Mean(const cell_wrap_0 R_input[2], boolean_T b_outlier_rejection,
                   double n_iterations, double thr_convergence, double R[9])
{
  double vectors_total[18];
  double s[9];
  double s_prev[9];
  double step_num[9];
  double b_s[3];
  double sorted_v_norms[2];
  double xv[2];
  double absxk;
  double d;
  double step_den;
  double t;
  double y;
  int b_i;
  int i;
  int j;
  int jA;
  int jy;
  signed char ipiv[3];
  boolean_T exitg2;
  boolean_T isodd;

  cell_wrap_0 R_result;

  if (!isInitialized_ChordalL1Mean) {
    ChordalL1Mean_initialize();
  }

  jy = 0;
  for (i = 0; i < 2; i++) {
    jy = i;
    std::memcpy(&vectors_total[i * 9], &R_input[i].f1[0], 9U * sizeof(double));
  }

  for (j = 0; j < 9; j++) {
    xv[0] = vectors_total[j];
    xv[1] = vectors_total[j + 9];
    jA = 0;
    int exitg1;
    do {
      exitg1 = 0;
      if (jA < 2) {
        if (rtIsNaN(xv[jA])) {
          s[j] = rtNaN;
          exitg1 = 1;
        } else {
          jA++;
        }
      } else {
        if (((xv[0] < 0.0) != (xv[1] < 0.0)) || rtIsInf(xv[0])) {
          s[j] = (xv[0] + xv[1]) / 2.0;
        } else {
          s[j] = xv[0] + (xv[1] - xv[0]) / 2.0;
        }

        exitg1 = 1;
      }
    } while (exitg1 == 0);
  }

  //  2. Optimize
  j = 0;
  exitg2 = false;
  while ((!exitg2) && (j <= static_cast<int>(n_iterations) - 1)) {
    double scale;
    for (jA = 0; jA < 9; jA++) {
      step_num[jA] = std::abs(vectors_total[jA + 9 * jy] - s[jA]);
    }

    y = step_num[0];
    for (jA = 0; jA < 8; jA++) {
      y += step_num[jA + 1];
    }

    if (y == 0.0) {
      coder::b_rand(step_num);
      for (b_i = 0; b_i < 9; b_i++) {
        s[b_i] += step_num[b_i] * 0.001;
      }
    }

    for (jy = 0; jy < 2; jy++) {
      y = 0.0;
      scale = 3.3121686421112381E-170;
      for (jA = 0; jA < 9; jA++) {
        absxk = std::abs(vectors_total[jA + 9 * jy] - s[jA]);
        if (absxk > scale) {
          t = scale / absxk;
          y = y * t * t + 1.0;
          scale = absxk;
        } else {
          t = absxk / scale;
          y += t * t;
        }
      }

      xv[jy] = scale * std::sqrt(y);
    }

    //  Compute the inlier threshold (if we reject outliers).
    absxk = rtInf;
    if (b_outlier_rejection) {
      sorted_v_norms[0] = xv[0];
      sorted_v_norms[1] = xv[1];
      coder::internal::sort(sorted_v_norms);
      absxk = sorted_v_norms[0];
      if (!(absxk > 1.356)) {
        absxk = 1.356;
      }

      //  2*sqrt(2)*sin(1/2) is approximately 1.356
    }

    std::memset(&step_num[0], 0, 9U * sizeof(double));
    step_den = 0.0;
    jy = 0;
    for (i = 0; i < 2; i++) {
      jy = i;
      if (!(xv[i] > absxk)) {
        for (b_i = 0; b_i < 9; b_i++) {
          step_num[b_i] += vectors_total[b_i + 9 * i] / xv[i];
        }

        step_den += 1.0 / xv[i];
      }
    }

    y = 0.0;
    scale = 3.3121686421112381E-170;
    for (jA = 0; jA < 9; jA++) {
      t = s[jA];
      d = step_num[jA] / step_den;
      s[jA] = d;
      t = d - t;
      absxk = std::abs(t);
      if (absxk > scale) {
        t = scale / absxk;
        y = y * t * t + 1.0;
        scale = absxk;
      } else {
        t = absxk / scale;
        y += t * t;
      }
    }

    y = scale * std::sqrt(y);
    if (y < thr_convergence) {
      exitg2 = true;
    } else {
      j++;
    }
  }

  isodd = true;
  for (jA = 0; jA < 9; jA++) {
    if ((!isodd) || (rtIsInf(s[jA]) || rtIsNaN(s[jA]))) {
      isodd = false;
    }
  }

  if (isodd) {
    coder::internal::svd(s, step_num, b_s, s_prev);
  } else {
    for (jy = 0; jy < 9; jy++) {
      step_num[jy] = rtNaN;
      s_prev[jy] = rtNaN;
    }
  }

  for (b_i = 0; b_i < 3; b_i++) {
    t = step_num[b_i];
    d = step_num[b_i + 3];
    absxk = step_num[b_i + 6];
    for (i = 0; i < 3; i++) {
      R[b_i + 3 * i] = (t * s_prev[i] + d * s_prev[i + 3]) + absxk * s_prev[i +
        6];
    }
  }

  std::memcpy(&s[0], &R[0], 9U * sizeof(double));
  ipiv[0] = 1;
  ipiv[1] = 2;
  for (j = 0; j < 2; j++) {
    int b_tmp;
    int ix;
    int jp1j;
    int mmj_tmp;
    mmj_tmp = 1 - j;
    b_tmp = j << 2;
    jp1j = b_tmp + 2;
    i = 3 - j;
    jy = 0;
    ix = b_tmp;
    absxk = std::abs(s[b_tmp]);
    for (jA = 2; jA <= i; jA++) {
      ix++;
      t = std::abs(s[ix]);
      if (t > absxk) {
        jy = jA - 1;
        absxk = t;
      }
    }

    if (s[b_tmp + jy] != 0.0) {
      if (jy != 0) {
        i = j + jy;
        ipiv[j] = static_cast<signed char>(i + 1);
        absxk = s[j];
        s[j] = s[i];
        s[i] = absxk;
        absxk = s[j + 3];
        s[j + 3] = s[i + 3];
        s[i + 3] = absxk;
        absxk = s[j + 6];
        s[j + 6] = s[i + 6];
        s[i + 6] = absxk;
      }

      b_i = (b_tmp - j) + 3;
      for (jy = jp1j; jy <= b_i; jy++) {
        s[jy - 1] /= s[b_tmp];
      }
    }

    jy = b_tmp + 3;
    jA = b_tmp;
    for (jp1j = 0; jp1j <= mmj_tmp; jp1j++) {
      absxk = s[jy];
      if (s[jy] != 0.0) {
        ix = b_tmp + 1;
        b_i = jA + 5;
        i = (jA - j) + 6;
        for (int ijA = b_i; ijA <= i; ijA++) {
          s[ijA - 1] += s[ix] * -absxk;
          ix++;
        }
      }

      jy += 3;
      jA += 3;
    }
  }

  isodd = (ipiv[0] > 1);
  y = s[0] * s[4] * s[8];
  if (ipiv[1] > 2) {
    isodd = !isodd;
  }

  if (isodd) {
    y = -y;
  }

  if (y < 0.0) {
    s_prev[6] = -s_prev[6];
    s_prev[7] = -s_prev[7];
    s_prev[8] = -s_prev[8];
    for (b_i = 0; b_i < 3; b_i++) {
      t = step_num[b_i];
      d = step_num[b_i + 3];
      absxk = step_num[b_i + 6];
      for (i = 0; i < 3; i++) {
        R[b_i + 3 * i] = (t * s_prev[i] + d * s_prev[i + 3]) + absxk * s_prev[i
          + 6];
      }
    }
  }

  for (int i = 0; i < 9; i++) {
      R_result.f1[i] = R[i];
  }

  return R_result;
}

//
// File trailer for ChordalL1Mean.cpp
//
// [EOF]
//
